// --------------------------------------------------------------------------------------------------------------------
// <copyright file="URControlClient.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.UniversalRobots
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Net;
    using System.Net.Sockets;
    using System.Text;
    using System.Threading;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.JointController;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Class for communicating with URControl server running on a Universal Robots controller.
    /// See documentation provided by Universal Robots for more information.
    /// </summary>
    public class URControlClient : IDisposable
    {
        /// <summary>
        /// Number of joints.  This value must always be 6 unless structs/classes associated
        /// with the communication protocol are also changed.
        /// </summary>
        public const int NumJoints = 6;

        /// <summary>
        /// Number of cycles between control updates (keeps us from flooding UR controller)
        /// </summary>
        private const int CyclesPerControlUpdate = 2;

        /// <summary>
        /// Default port for a URControl server to listen on.
        /// </summary>
        private const int DefaultURControlServerPort = 30002;

        /// <summary>
        /// Default port to listen on for real-time updates
        /// </summary>
        private const int DefaultURRealTimePort = 30003;

        /// <summary>
        /// Default timeout for opening TCP connection.
        /// </summary>
        private const int DefaultConnectTimeoutInMilliseconds = 5000;

        /// <summary>
        /// Size of the buffer to use for reading packets from TCP connection
        /// </summary>
        private const int TcpReadBufferSize = 2048;

        /// <summary>
        /// Expected size of the packet received from UR real-time interface.
        /// </summary>
        private const int ExpectedSizeOfRealtimePacket = 812;

        /// <summary>
        /// Default joint acceleration in rad/s^2
        /// </summary>
        private const double DefaultJointAcceleration = 7;

        /// <summary>
        /// Default timeout for joint velocity commands in seconds.
        /// </summary>
        private const double DefaultJointVelocityCommandTimeout = 0.3;

        /// <summary>
        /// Manages connection to URControl server.
        /// </summary>
        private IURControlClientNetworkConnection urControlServerClient;

        /// <summary>
        /// Manages connection to real time interface
        /// </summary>
        private IURControlClientNetworkConnection urRealTimeInterface;

        /// <summary>
        /// Network stream for sending and receiving data via urControlServerClient
        /// </summary>
        private Stream clientNetworkStream;

        /// <summary>
        /// Network stream for real-time interface
        /// </summary>
        private Stream realTimeNetworkStream;

        /// <summary>
        /// Hostname of URControl server.
        /// </summary>
        private string hostName;

        /// <summary>
        /// Port to connect to URControl server on.
        /// </summary>
        private int controlServerPort;

        /// <summary>
        /// Port to connect to real time data publisher on.
        /// </summary>
        private int realTimePort;

        /// <summary>
        /// Indicates whether or not instance has been disposed.
        /// </summary>
        private bool disposed = false;

        /// <summary>
        /// Cancellation token source used to cancel reading task.
        /// </summary>
        private CancellationTokenSource cts;

        /// <summary>
        /// Task for processing feedback.
        /// </summary>
        private Task processFeedbackTask;

        /// <summary>
        /// Current robot mode package
        /// </summary>
        private volatile ControllerData controllerData = null;

        /// <summary>
        /// Current joint chain data
        /// </summary>
        private volatile JointChainData jointChainData = null;

        /// <summary>
        /// Current TCP force data
        /// </summary>
        private volatile TcpForceData tcpForceData = null;

        /// <summary>
        /// Current TCP force data
        /// </summary>
        private volatile CartesianData tcpCartesianData = null;

        /// <summary>
        /// Current configuration data
        /// </summary>
        private volatile ConfigurationData configurationData = null;

        /// <summary>
        /// Current additional info
        /// </summary>
        private volatile AdditionalInfo additionalInfo = null;

        /// <summary>
        /// Current master board data.
        /// </summary>
        private volatile MasterboardData masterboardData = null;

        /// <summary>
        /// Current real-time data.
        /// </summary>
        private volatile RealtimeData realtimeData = null;

        /// <summary>
        /// Lock object for sending commands
        /// </summary>
        private object sendCommandLockObject;

        /// <summary>
        /// Initializes a new instance of the URControlClient class.
        /// </summary>
        /// <param name="hostName">Hostname of the URControl server to connect to.</param>
        /// <param name="controlServerPort">Port the URControl server is listening on.</param>
        /// <param name="realTimePort">Port that real time data stream is being published on.</param>
        public URControlClient(string hostName, int controlServerPort = DefaultURControlServerPort, int realTimePort = DefaultURRealTimePort)
        {
            this.Initialize(hostName, controlServerPort, realTimePort);

            this.urControlServerClient = new URControlClientTcpConnection();
            this.urRealTimeInterface = new URControlClientTcpConnection();
        }

        /// <summary>
        /// Initializes a new instance of the URControlClient class.
        /// </summary>
        /// <param name="urControlClientNetworkConnection"> urControlClientNetworkConnection to use for UR Control Server interface.</param>
        /// <param name="urRealtimeNetworkConnection"> urControlClientNetworkConnection to use for real time interface.</param>
        internal URControlClient(
            IURControlClientNetworkConnection urControlClientNetworkConnection,
            IURControlClientNetworkConnection urRealtimeNetworkConnection)
        {
            this.Initialize(string.Empty, 0, 0);

            this.urControlServerClient = urControlClientNetworkConnection;
            this.urRealTimeInterface = urRealtimeNetworkConnection;
        }

        /// <summary>
        /// Gets a value indicating whether or not the client is connected to a URControl server.
        /// </summary>
        public bool IsConnected
        {
            get
            {
                return (this.urControlServerClient != null
                    && this.urControlServerClient.Connected
                    && this.clientNetworkStream != null
                    && this.clientNetworkStream.CanWrite
                    && this.clientNetworkStream.CanRead
                    && this.urRealTimeInterface != null
                    && this.urRealTimeInterface.Connected
                    && this.realTimeNetworkStream != null
                    && this.realTimeNetworkStream.CanRead);
            }
        }

        /// <summary>
        /// Gets the current robot mode data, value is null if no data yet available
        /// </summary>
        public ControllerData ControllerData
        {
            get
            {
                return this.controllerData;
            }
        }

        /// <summary>
        /// Gets the current joint data, value is null if no data yet available
        /// </summary>
        public JointChainData JointChainData
        {
            get
            {
                return this.jointChainData;
            }
        }

        /// <summary>
        /// Gets the current TCP force data, value is null if no data yet available
        /// </summary>
        public TcpForceData TcpForceData
        {
            get
            {
                return this.tcpForceData;
            }
        }

        /// <summary>
        /// Gets the current Cartesian position data, value is null if no data yet available
        /// </summary>
        public CartesianData TcpCartesianData
        {
            get
            {
                return this.tcpCartesianData;
            }
        }

        /// <summary>
        /// Gets the current configuration data, value is null if no data yet available
        /// </summary>
        public ConfigurationData ConfigurationData
        {
            get
            {
                return this.configurationData;
            }
        }

        /// <summary>
        /// Gets the current additional info, value is null if no data yet available
        /// </summary>
        public AdditionalInfo AdditionalInfo
        {
            get
            {
                return this.additionalInfo;
            }
        }

        /// <summary>
        /// Gets the current master board data, value is null if no data yet available
        /// </summary>
        public MasterboardData MasterboardData
        {
            get
            {
                return this.masterboardData;
            }
        }

        /// <summary>
        /// Gets the current real-time data package.
        /// </summary>
        public RealtimeData RealtimeData
        {
            get
            {
                return this.realtimeData;
            }
        }

        /// <summary>
        /// Gets or sets the current joint controller.
        /// </summary>
        public JointController JointController { get; set; }

        /// <summary>
        /// Gets the current manipulator status
        /// </summary>
        public ManipulatorStatus ManipulatorStatus
        {
            get
            {
                return this.BuildManipulatorStatus();
            }
        }

        /// <summary>
        /// Release resources.
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Synchronously connect.
        /// </summary>
        /// <param name="timeoutInMilliseconds">Timeout in milliseconds.  -1 for infinite timeout.</param>
        /// <returns>True if connection completed.  False if timeout occurs waiting for connection.</returns>
        public bool Connect(int timeoutInMilliseconds = DefaultConnectTimeoutInMilliseconds)
        {
            if (!this.OpenConnections(timeoutInMilliseconds))
            {
                return false;
            }

            this.StartControlLoop();
            return true;
        }

        /// <summary>
        /// Send a command to the UR Control Server
        /// </summary>
        /// <param name="command">Command to send. </param>
        /// <returns>True if command sent (does not indicate success or failure of command execution).</returns>
        public bool SendCommand(string command)
        {
            return this.SendCommand(command, this.clientNetworkStream);
        }

        /// <summary>
        /// Send a single line command to the UR controller
        /// </summary>
        /// <param name="command">Command to send. </param>
        /// <param name="stream"> Stream to write command to. </param>
        /// <returns>True if command sent (does not indicate success or failure of command execution).</returns>
        private bool SendCommand(string command, Stream stream)
        {
            if (stream != null && stream.CanWrite)
            {
                byte[] data = System.Text.Encoding.ASCII.GetBytes(command + "\n");
                lock (this.sendCommandLockObject)
                {
                    stream.Write(data, 0, data.Length);
                }

                return true;
            }

            return false;
        }

        /// <summary>
        /// Open network connections
        /// </summary>
        /// <param name="timeoutInMilliseconds">Timeout for opening connections.</param>
        /// <returns>true if success.</returns>
        private bool OpenConnections(int timeoutInMilliseconds)
        {
            Task task = this.urControlServerClient.ConnectAsync(this.hostName, this.controlServerPort);
            try
            {
                if (task.Wait(timeoutInMilliseconds))
                {
                    this.clientNetworkStream = this.urControlServerClient.GetStream();
                }
                else
                {
                    TraceOut.Error(TraceContexts.HW, "Could not connect to UR Control Server at {0}:{1}", this.hostName, this.controlServerPort);
                    return false;
                }
            }
            catch (AggregateException e)
            {
                // A single TaskCanceled exception is expected during shutdown of the client.
                // Any other combinations of exceptions is un-handled here.
                if (e.InnerExceptions.Count > 1 || !(e.InnerException is TaskCanceledException))
                {
                    throw;
                }

                TraceOut.Info(TraceContexts.HW, "Connection to UR Control Server at {0}:{1} canceled", this.hostName, this.controlServerPort);
                return false;
            }

            task = this.urRealTimeInterface.ConnectAsync(this.hostName, this.realTimePort);
            try
            {
                if (task.Wait(timeoutInMilliseconds))
                {
                    this.realTimeNetworkStream = this.urRealTimeInterface.GetStream();
                }
                else
                {
                    TraceOut.Error(TraceContexts.HW, "Could not connect to UR real-time interface at {0}:{1}", this.hostName, this.realTimePort);
                    return false;
                }
            }
            catch (AggregateException e)
            {
                // A single TaskCanceled exception is expected during shutdown of the client.
                // Any other combinations of exceptions is un-handled here.
                if (e.InnerExceptions.Count > 1 || !(e.InnerException is TaskCanceledException))
                {
                    throw;
                }

                TraceOut.Info(TraceContexts.HW, "Connection to UR real time interface at {0}:{1} canceled", this.hostName, this.realTimePort);
                return false;
            }

            return true;
        }

        /// <summary>
        /// Starts the main control loop task. 
        /// </summary>
        private void StartControlLoop()
        {
            this.processFeedbackTask = new Task((obj) => this.ProcessFeedback(obj), this.cts.Token);
            this.processFeedbackTask.Start();
        }

        /// <summary>
        /// Release resources.
        /// </summary>
        /// <param name="disposing">Indicates whether or not Dispose() was called.</param>
        private void Dispose(bool disposing)
        {
            this.cts.Cancel();

            if (this.processFeedbackTask != null)
            {
                try
                {
                    this.processFeedbackTask.Wait();
                }
                catch (AggregateException e)
                {
                    // A TaskCanceled or InvalidOperationExceptions may be expected during shutdown of the client.
                    // Any other combinations of exceptions is un-handled here.
                    foreach (Exception innerEx in e.InnerExceptions)
                    {
                        if (!(innerEx is TaskCanceledException || innerEx is InvalidOperationException))
                        {
                            throw;
                        }
                    }
                }
            }

            // only dispose of managed resources if disposing is true
            if (disposing)
            {
                if (!this.disposed)
                {
                    this.processFeedbackTask.Dispose();
                    this.cts.Dispose();

                    if (this.urControlServerClient != null)
                    {
                        this.urControlServerClient.Close();
                        this.urRealTimeInterface.Close();
                        this.urControlServerClient = null;
                    }

                    this.disposed = true;
                }
            }
        }

        /// <summary>
        /// Listens for messages from the URControl server, parses them and 
        /// updates properties of <c>URControlClient</c> appropriately.
        /// Parameter should be a <c>CancellationToken</c>.
        /// </summary>
        /// <param name="obj">A cancellation token.</param>
        private void ProcessFeedback(object obj)
        {
            byte[] controlServerBuf = new byte[TcpReadBufferSize];
            byte[] realtimeInterfaceBuf = new byte[812];

            CancellationToken token = (CancellationToken)obj;

            int controlServerBytesRead;
            int realtimeBytesRead;
            int realtimePositionInPacket = 0;

            bool possiblyBadLastControlServerPacket = false;

            Task<int> controlServerReadTask = this.clientNetworkStream.ReadAsync(controlServerBuf, 0, controlServerBuf.Length, token);

            this.realTimeNetworkStream.ReadTimeout = 1000;

            int controllerCycleCount = 0;

            while (!token.IsCancellationRequested)
            {
                try
                {
                    realtimeBytesRead = this.realTimeNetworkStream.Read(
                        realtimeInterfaceBuf,
                        realtimePositionInPacket,
                        realtimeInterfaceBuf.Length - realtimePositionInPacket);
                }
                catch (IOException)
                {
                    if (token.IsCancellationRequested)
                    {
                        break;
                    }

                    // inelegant solution - but only known fix
                    // for some reason connections can become corrupted
                    // only seen when stopping task in debugger
                    this.ResetConnections();
                    continue;
                }

                // 0 bytes read indicates a closed connection
                if (realtimeBytesRead == 0)
                {
                    break;
                }

                if (realtimeBytesRead + realtimePositionInPacket == ExpectedSizeOfRealtimePacket)
                {
                    // ignore partial packets
                    this.ProcessRealtimeInterfacePacket(realtimeInterfaceBuf, realtimeBytesRead + realtimePositionInPacket);

                    // start over reading a new packet
                    realtimePositionInPacket = 0;
                }

                if (controlServerReadTask.IsCompleted)
                {
                    controlServerBytesRead = controlServerReadTask.Result;

                    // 0 bytes read indicates a closed connection
                    if (controlServerBytesRead == 0)
                    {
                        break;
                    }

                    // if buffer is full, we probably truncated a packet (and may have dropped data?)
                    // discard the next packet as well since it is likely to start with a partial.
                    // The URControl server does not use packet header and footers, so this is difficult
                    // to detect/recover from more gracefully.
                    if (controlServerBytesRead == TcpReadBufferSize)
                    {
                        possiblyBadLastControlServerPacket = true;
                    }
                    else if (possiblyBadLastControlServerPacket)
                    {
                        possiblyBadLastControlServerPacket = false;
                    }
                    else
                    {
                        this.ProcessControlServerPacket(controlServerBuf, controlServerBytesRead);
                    }

                    controlServerReadTask = this.clientNetworkStream.ReadAsync(controlServerBuf, 0, controlServerBuf.Length, token);
                }

                // execute current controller if any
                JointController jointController;
                if ((jointController = this.JointController) != null)
                {
                    if (controllerCycleCount++ >= CyclesPerControlUpdate)
                    {
                        controllerCycleCount = 0;

                        double[] output = jointController.Cycle(this.BuildManipulatorStatus());

                        string commandString = string.Empty;
                        switch (jointController.ControlOutputSignalType)
                        {
                            case JointSignalType.Velocity:
                                commandString = URControlCommandBuilder.ConstructJointVelocityString(
                                    output,
                                    DefaultJointAcceleration,
                                    DefaultJointVelocityCommandTimeout);
                                break;
                            case JointSignalType.Position:
                                commandString = URControlCommandBuilder.ConstructJointPositionGoalString(output);
                                break;
                            default:
                                TraceOut.Warning(
                                    TraceContexts.HW,
                                    "Unsupported control reference type for UR: {0}",
                                    jointController.ControlOutputSignalType);
                                break;
                        }

                        if (!string.IsNullOrEmpty(commandString))
                        {
                            this.SendCommand(commandString, this.realTimeNetworkStream);
                        }
                    }
                }
            }
        }

        /// <summary>
        /// Process a packet from the UR Control Server
        /// </summary>
        /// <param name="buf">Packet buffer.</param>
        /// <param name="dataLength">Length of data in buffer.</param>
        private void ProcessControlServerPacket(byte[] buf, int dataLength)
        {
            // Extract only the last packet (not package) from message, assume that only
            // most recent data is useful and is contained the second packet.
            int[] packetStartOffests = URDataByteOperations.GetPacketStartOffsets(buf, dataLength);

            if (packetStartOffests == null)
            {
                // indicates an error
                return;
            }

            int lastPacketLength = dataLength - packetStartOffests[packetStartOffests.Length - 1];
            byte[] lastPacket = new byte[lastPacketLength];

            Array.Copy(buf, packetStartOffests[packetStartOffests.Length - 1], lastPacket, 0, lastPacketLength);

            int currentOffset = 0;
            Tuple<URDataPackages.PackageTypes, int> typeAndOffset;

            // Iterate over the last packet, extracting each package in sequence and updating relevant property.
            while (true)
            {
                typeAndOffset = URDataByteOperations.GetNextTypeAndOffset(lastPacket, currentOffset, lastPacketLength);

                if (typeAndOffset == null)
                {
                    break;
                }

                currentOffset = typeAndOffset.Item2;
                switch (typeAndOffset.Item1)
                {
                    case URDataPackages.PackageTypes.RobotModeData:
                        this.controllerData = new ControllerData(buf, currentOffset);
                        break;
                    case URDataPackages.PackageTypes.JointData:
                        this.jointChainData = new JointChainData(buf, currentOffset, NumJoints);
                        break;
                    case URDataPackages.PackageTypes.ForceModeData:
                        this.tcpForceData = new TcpForceData(buf, currentOffset);
                        break;
                    case URDataPackages.PackageTypes.CartesianInfo:
                        this.tcpCartesianData = new CartesianData(buf, currentOffset);
                        break;
                    case URDataPackages.PackageTypes.ConfigurationData:
                        this.configurationData = new ConfigurationData(buf, currentOffset);
                        break;
                    case URDataPackages.PackageTypes.AdditionalInfo:
                        this.additionalInfo = new AdditionalInfo(buf, currentOffset);
                        break;
                    case URDataPackages.PackageTypes.MasterboardData:
                        this.masterboardData = new MasterboardData(buf, currentOffset);
                        break;
                }
            }
        }

        /// <summary>
        /// Process a packet from the UR Real-time Interface
        /// </summary>
        /// <param name="buf">Packet buffer.</param>
        /// <param name="dataLength">Length of data in buffer.)</param>
        private void ProcessRealtimeInterfacePacket(byte[] buf, int dataLength)
        {
            if (dataLength != ExpectedSizeOfRealtimePacket)
            {
                TraceOut.Error(TraceContexts.HW, "Expected {0} bytes from UR real-time interface.  Received: {1} bytes.", ExpectedSizeOfRealtimePacket, dataLength);
                return;
            }

            RealtimeData packet = new RealtimeData(buf, 0);

            if (packet.RealtimeDataPackage.MessageLength != ExpectedSizeOfRealtimePacket)
            {
                TraceOut.Warning(
                    TraceContexts.HW,
                    "Incorrect message length reported in real time data packet, got {0}, expected {1}",
                    packet.RealtimeDataPackage.MessageLength,
                    ExpectedSizeOfRealtimePacket);

                return;
            }

            this.realtimeData = packet;
        }

        /// <summary>
        /// Initialize all values to defaults
        /// </summary>
        /// <param name="hostname">The hostname.</param>
        /// <param name="controlServerPort">Control server port number.</param>
        /// <param name="realTimePort"> Real time port number.</param>
        private void Initialize(string hostname, int controlServerPort, int realTimePort)
        {
            this.hostName = hostname;
            this.controlServerPort = controlServerPort;
            this.realTimePort = realTimePort;
            this.clientNetworkStream = null;
            this.cts = new CancellationTokenSource();
            this.processFeedbackTask = null;
            this.JointController = null;
            this.sendCommandLockObject = new object();
        }

        /// <summary>
        /// Get manipulator status for UR arm
        /// </summary>
        /// <returns>Manipulator status.</returns>
        private ManipulatorStatus BuildManipulatorStatus()
        {
            if (this.RealtimeData == null)
            {
                // data not ready
                return null;
            }

            RealtimeDataPackage rtData = this.RealtimeData.RealtimeDataPackage;

            double[] tcpForce = 
            {
                rtData.TcpForceX,
                rtData.TcpForceY,
                rtData.TcpForceZ,
                rtData.TcpForceRX,
                rtData.TcpForceRY,
                rtData.TcpForceRZ
            };

            double[] jointPositions = new double[NumJoints];
            double[] jointVelocities = new double[NumJoints];
            double[] jointCommandedPositions = new double[NumJoints];

            jointCommandedPositions[0] = rtData.QTarget0;
            jointCommandedPositions[1] = rtData.QTarget1;
            jointCommandedPositions[2] = rtData.QTarget2;
            jointCommandedPositions[3] = rtData.QTarget3;
            jointCommandedPositions[4] = rtData.QTarget4;
            jointCommandedPositions[5] = rtData.QTarget5;

            jointPositions[0] = rtData.QActual0;
            jointPositions[1] = rtData.QActual1;
            jointPositions[2] = rtData.QActual2;
            jointPositions[3] = rtData.QActual3;
            jointPositions[4] = rtData.QActual4;
            jointPositions[5] = rtData.QActual5;

            jointVelocities[0] = rtData.QdActual0;
            jointVelocities[1] = rtData.QdActual1;
            jointVelocities[2] = rtData.QdActual2;
            jointVelocities[3] = rtData.QdActual3;
            jointVelocities[4] = rtData.QdActual4;
            jointVelocities[5] = rtData.QdActual5;

            Vector3 rotVec = new Vector3(rtData.TcpPositionRX, rtData.TcpPositionRY, rtData.TcpPositionRZ);

            double angle = rotVec.Length();
            rotVec = Vector3.Normalize(rotVec);

            Pose tcpPose = new Pose(
                new Vector3(
                    rtData.TcpPositionX,
                    rtData.TcpPositionY,
                    rtData.TcpPositionZ),
                Quaternion.FromAxisAngle(
                    new AxisAngle(
                        rotVec,
                        angle)));

            return new ManipulatorStatus(
                rtData.Timestamp,
                tcpPose,
        new Pose(),
                jointPositions,
                jointVelocities,
                jointCommandedPositions,
                null,
                tcpForce,
                null);
        }

        /// <summary>
        /// Reset connections if something goes wrong
        /// </summary>
        private void ResetConnections()
        {
            this.realTimeNetworkStream.Close();
            this.urRealTimeInterface.Close();
            this.urRealTimeInterface.Dispose();
            this.clientNetworkStream.Close();
            this.urControlServerClient.Close();
            this.urControlServerClient.Dispose();

            this.urControlServerClient = new URControlClientTcpConnection();
            this.urRealTimeInterface = new URControlClientTcpConnection();

            this.OpenConnections(DefaultConnectTimeoutInMilliseconds);
        }
    }
}
